package mainFusion;

import odometerDevice.connectionDevice.android.SansungI9000;
import odometerDevice.connectionDevice.mindstorms.NXTConnection;
import odometerDevice.device.AndroidDevice;
import odometerDevice.device.IOdometry;
import odometerDevice.device.MindstormsDevice;
import fusionMethod.RadialBasisFunctionNN;

public class OdometryFusionManager {

	public static void main(String[] args) throws InterruptedException {

		System.out.println();
		System.out.println(">>>>>>>>>>");
		System.out.println();

		boolean training = false;
		final RadialBasisFunctionNN radialBasisNN;

		MindstormsDevice mindstormsDevice = new MindstormsDevice();
		mindstormsDevice.configureDevice(new NXTConnection());

		Thread nxtThread = new Thread(mindstormsDevice);
		nxtThread.start();

		if (training) {

			AndroidDevice androidDevice = new AndroidDevice();
			androidDevice.configureDevice(new SansungI9000());

			Thread androidThread = new Thread(androidDevice);
			androidThread.start();

			radialBasisNN = new RadialBasisFunctionNN(androidDevice, androidDevice, mindstormsDevice, false);
		} else {

			radialBasisNN = new RadialBasisFunctionNN(mindstormsDevice);

		}

		radialBasisNN.calibrate();

		double x = 0;
		double y = 0;
		double orientation = 0;
		int samplingTimeStep = 1;
		double[] position = null;

		Thread.sleep(5000);

		while (samplingTimeStep < 100) {

			mindstormsDevice.setRotation(20, 20, 20, 0);
			Thread.sleep(3000);

			position = radialBasisNN.compute(x, y, 1, orientation, 0, 20);

			x = position[0];
			y = position[1];
			orientation = position[2];

			samplingTimeStep++;

		}

	}

	private void moveFoward(IOdometry odometer, RadialBasisFunctionNN radialBasisNN, double[] actualPosition, double goalX, double goalY) throws InterruptedException {

		odometer.setRotation(50, 50, 20, 20);
		Thread.sleep(3000);

		actualPosition = radialBasisNN.compute(actualPosition[0], actualPosition[1], 1, actualPosition[2], 20, 20);

	}

	private void turn(IOdometry odometer, double[] actualPosition, int goalDegree) {

	}
}
